package org.opentcs.strategies.basic.routing.jgrapht.jjwcalg;

import lombok.AllArgsConstructor;
import lombok.Data;
import lombok.Setter;
import org.jgrapht.Graph;
import org.jgrapht.GraphPath;
import org.jgrapht.Graphs;
import org.jgrapht.alg.shortestpath.ListSingleSourcePathsImpl;
import org.jgrapht.graph.GraphWalk;
import org.opentcs.components.kernel.routing.Edge;
import org.opentcs.data.model.Path;
import org.opentcs.data.model.TCSResourceReference;
import org.opentcs.strategies.basic.scheduling.ReservationPool;

import java.util.*;

/**
 * 自己编写的A*算法
 */
public class MyAStartAlg<V, E> implements JJWCShortestPathAlgorithm<String, Edge> {

  private PriorityQueue<Node> openNodeList;
  /**
   * 这是表示已经访问过了的节点名字
   */
  private Set<String> closeList;
  /**
   * 这是加重权重的基础权值，100m
   */
  private long firstBaseCost = 100000;
  //一次权重叠加为10m
  private long baseCost = 10000;


  /**
   * 地图对象
   */
  private Graph<String, Edge> graph;

  private Map<String, Edge> edgeList;

  private Map<String, Double> edgeAddWightList;

  private Map<String, Double> nodeGn;
  private Map<String, Node> nodeFn;

  private final ReservationPool reservationPool;

  @Setter
  private String vehicleName;

  public MyAStartAlg(Graph<String, Edge> graph, ReservationPool reservationPool) {
    this.graph = graph;
    this.reservationPool = reservationPool;
  }

  private void init() {
    if (openNodeList == null) {
      openNodeList = new PriorityQueue<>(new Comparator<Node>() {

        @Override
        public int compare(Node o1, Node o2) {
          return (int) (o1.getCost() - o2.getCost());
        }
      });
    }
    openNodeList.clear();
    if (closeList == null) {
      closeList = new HashSet<>();
    }
    closeList.clear();
    if (edgeList == null) {
      edgeList = new HashMap<>();
    }
    edgeList.clear();
    if (nodeGn == null) {
      nodeGn = new HashMap<>();
    }
    nodeGn.clear();

    if (nodeFn == null) {
      nodeFn = new HashMap<>();
    }
    nodeFn.clear();
    if (edgeAddWightList == null)
      edgeAddWightList = new HashMap<>();
    edgeAddWightList.clear();

  }


  public GraphPath<String, Edge> getPath(String sourceVertex, String targetVertex) {
    if (this.graph.containsVertex(sourceVertex) && this.graph.containsVertex(targetVertex)) {
      init();
      //如果起点和终点相同
      if (sourceVertex.equals(targetVertex)) {
        return createEmptyPath(sourceVertex, targetVertex);
      }
      nodeGn.put(sourceVertex, 0.0);
      //todo:这里进行第一次的特殊处理
      findFirstNeighbors(sourceVertex, targetVertex);
      closeList.add(sourceVertex);
      while (!openNodeList.isEmpty()) {
        Node minNode = openNodeList.poll();
        if (minNode.getNodeName().equals(targetVertex))
          return createShortPath(sourceVertex, targetVertex, minNode);
        findNeighbors(minNode, targetVertex);
        closeList.add(minNode.getNodeName());
      }
      return createEmptyPath(sourceVertex, targetVertex);
    } else {
      throw new IllegalArgumentException("Source vertex '" + sourceVertex + "' does not exist");
    }
  }


  @Override
  public double getPathWeight(String source, String sink) {
    GraphPath<String, Edge> p = this.getPath(source, sink);
    return p == null ? Double.POSITIVE_INFINITY : p.getWeight();
  }

  @Override
  public SingleSourcePaths<String, Edge> getPaths(String source) {
    if (!this.graph.containsVertex(source)) {
      throw new IllegalArgumentException("graph must contain the source vertex");
    } else {
      Map<String, GraphPath<String, Edge>> paths = new HashMap<>();

      for (String v : this.graph.vertexSet()) {
        paths.put(v, this.getPath(source, v));
      }

      return new ListSingleSourcePathsImpl<>(this.graph, source, paths);
    }
  }


  private GraphPath<String, Edge> createShortPath(String sourceVertex, String targetVertex, Node minNode) {
    List<Edge> edgeList = new ArrayList<>();
    List<String> vertexList = new ArrayList<>();
    vertexList.add(targetVertex);
    String v = targetVertex;
    double cost = 0;
    while (!v.equals(sourceVertex)) {
      Edge edge = this.edgeList.get(v);
      edgeList.add(edge);
      cost += edgeAddWightList.get(edge.getPath().getName());
      v = Graphs.getOppositeVertex(this.graph, this.edgeList.get(v), v);
      vertexList.add(v);
    }

    Collections.reverse(edgeList);
    Collections.reverse(vertexList);
    GraphWalk<String, Edge> stringEdgeGraphWalk = new GraphWalk<>(this.graph, sourceVertex, targetVertex, vertexList, edgeList, minNode.getCost());
    stringEdgeGraphWalk.setWeight(cost);
    return stringEdgeGraphWalk;
  }

  /**
   * 重路由的第一次路径进行特殊处理
   * @param sourceVertex
   * @param targetVertex
   */
  private void findFirstNeighbors(String sourceVertex, String targetVertex) {
    Set<Edge> edges = graph.outgoingEdgesOf(sourceVertex);
    for (Edge edge : edges) {
      String neighborVertex = edge.getTargetVertex();
      //如果已经访问过了那么就放弃访问
      if (closeList.contains(neighborVertex))
        continue;
      //过滤掉当前阻塞的资源
      if (reservationPool.getBlockSourceByVehicleName(vehicleName).contains(edge.getPath().getName()))
        continue;
      //添加权重
      Double preCost = nodeGn.get(sourceVertex);
      Double edgeWeight = 0.0;
      //是否会形成新的环
      if (reservationPool.willCaseRingBlock(Set.of(edge.getPath()), vehicleName))
        edgeWeight += firstBaseCost * 5;
      Edge edge1 = graph.getEdge(edge.getTargetVertex(), edge.getSourceVertex());
      Set<TCSResourceReference<?>> claimsMap = reservationPool.getClaimsMap(vehicleName);
      if (edge1 != null &&
          claimsMap.stream().anyMatch(var -> var.getName().equals(edge1.getPath().getName()))) {
        edgeWeight += 10 * firstBaseCost;
      }
      Double gCost = graph.getEdgeWeight(edge) + preCost + edgeWeight;

      Double fCost = gCost + getHCost(edge, targetVertex);
      nodeGn.put(neighborVertex, gCost);
      Node node = new Node(neighborVertex, fCost);
      openNodeList.offer(node);
      nodeFn.put(neighborVertex, node);
      edgeList.put(neighborVertex, edge);
      edgeAddWightList.put(edge.getPath().getName(), fCost);
    }
  }


  /**
   * 获取到当前最近节点的所可直通的节点
   *
   * @param minNode
   * @param targetVertex:终点
   */
  private void findNeighbors(Node minNode, String targetVertex) {
    String value = minNode.getNodeName();
    Set<Edge> edges = graph.outgoingEdgesOf(value);
    for (Edge edge : edges) {
      String neighborVertex = edge.getTargetVertex();
      //如果已经访问过了那么就放弃访问
      if (closeList.contains(neighborVertex))
        continue;
      //过滤掉当前阻塞的资源
      if (reservationPool.getBlockSourceByVehicleName(vehicleName).contains(edge.getPath().getName()))
        continue;
      if (!value.equals(neighborVertex)) {
        Double preCost = nodeGn.get(value);
        Double gCost = graph.getEdgeWeight(edge) + preCost + getVehicleAddWeight(edge);
        Double fCost = gCost + getHCost(edge, targetVertex);
        //如果已经加入到了openlist当中那么看看是否需要进行更新
        if (nodeFn.containsKey(neighborVertex)) {
          Node fn = nodeFn.get(neighborVertex);
          //如果现在这个值比之前的值更小，那么就更新openlist当中的堆数据。（引用传递更新）
          if (fCost < fn.getCost()) {
            openNodeList.remove(fn);
            Node node = new Node(neighborVertex, fCost);
            openNodeList.offer(node);
            nodeFn.put(neighborVertex, node);
            edgeList.put(neighborVertex, edge);
            edgeAddWightList.put(edge.getPath().getName(), fCost);
          }
        } else {
          nodeGn.put(neighborVertex, gCost);
          Node node = new Node(neighborVertex, fCost);
          openNodeList.offer(node);
          nodeFn.put(neighborVertex, node);
          edgeList.put(neighborVertex, edge);
          edgeAddWightList.put(edge.getPath().getName(), fCost);
        }
      }
      //出现自环不进行处理
    }
  }

  private double getVehicleAddWeight(Edge edge) {
    double cost = 0.0;
    if (vehicleName == null)
      return 0;
    Set<TCSResourceReference<?>> claimsMap = reservationPool.getClaimsMap(vehicleName);
    Edge edge1 = graph.getEdge(edge.getTargetVertex(), edge.getSourceVertex());
    if (edge1 != null &&
        claimsMap.stream().anyMatch(var -> var.getName().equals(edge1.getPath().getName()))) {
      int size = 0;
      String targetVertex = edge.getSourceVertex();
      //判断形成互堵的步骤是在已有的路线的多少步骤。
      while (targetVertex != null && closeList.contains(targetVertex)) {
        Edge edge2 = edgeList.get(targetVertex);
        if (edge2 == null || size >= 10)
          break;
        size++;
        targetVertex = edge2.getSourceVertex();
      }
      cost += (12 - size) * baseCost;
    }
    return cost;
  }

  /**
   * 这是计算估计值的函数
   *
   * @param edge:当前添加到openlist当中的一条路径
   * @param targetVertex：这是整个路径搜寻中的终点名字
   * @return
   */
  private Double getHCost(Edge edge, String targetVertex) {
    return 0.0;
  }

  protected final GraphPath<String, Edge> createEmptyPath(String source, String sink) {
    return source.equals(sink) ? GraphWalk.singletonWalk(this.graph, source, 0.0) : null;
  }

  @Override
  public GraphPath<String, Edge> getPath(String var1, String var2, Map<String, Double> weights) {
    return null;
  }


  @Data
  @AllArgsConstructor
  private static class Node {
    private String nodeName;
    private Double cost;
  }
}


